from math import pi
import time

from std_msgs.msg import Header
from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3
from moveit.task_constructor import core, stages

import rclcpp


def main():
    rclcpp.init()
    node = rclcpp.Node("mtc_tutorial")

    arm_group = "arm"

    # Cartesian and joint-space interpolation planners
    cartesian = core.CartesianPath()
    joint_space = core.JointInterpolationPlanner()

    task = core.Task()
    task.name = "cartesian"
    task.loadRobotModel(node)

    # start from current robot state
    task.add(stages.CurrentState("current state"))

    # 同时执行x+0.2, y-0.3, rz+45°
    # move = stages.MoveRelative("combined x+y+rz", cartesian)
    # move.arm_group = arm_group
    # header = Header(frame_id="world")
    # move.setDirection(TwistStamped(
    #     header=header,
    #     twist=Twist(
    #         linear=Vector3(x=0.1, y=-0.1, z=0.0),    # 线速度
    #         angular=Vector3(x=0.0, y=0.0, z=pi/4)    # 角速度
    #     )
    # ))
    # task.add(move)

    # moveTo named posture, using joint-space interplation
    move = stages.MoveTo("moveTo rest_pose", joint_space)
    move.group = arm_group
    move.setGoal("rest_pose")
    task.add(move)

    move = stages.MoveTo("moveTo stand_pose", joint_space)
    move.group = arm_group
    move.setGoal("stand_pose")
    task.add(move)



    # 同时执行x+0.2, y-0.3, rz+45°
    # move = stages.MoveRelative("combined x+y+rz", cartesian)
    # move.arm_group = arm_group
    # header = Header(frame_id="world")
    # move.setDirection(TwistStamped(
    #     header=header,
    #     twist=Twist(
    #         linear=Vector3(x=0, y=-0.1, z=0.0),    # 线速度
    #         angular=Vector3(x=0.0, y=0.0, z=pi/4)    # 角速度
    #     )
    # ))
    # task.add(move)


    if task.plan():
        task.publish(task.solutions[0])


    hand_group = "hand"

    joint_space = core.JointInterpolationPlanner()

    task = core.Task()
    task.name = "hand_open_close"
    task.loadRobotModel(node)

    # start from current robot state
    task.add(stages.CurrentState("current state"))


    move = stages.MoveTo("moveTo open_pose", joint_space)
    move.group = hand_group
    move.setGoal("open_pose")
    task.add(move)


    move = stages.MoveTo("moveTo close_pose", joint_space)
    move.group = hand_group
    move.setGoal("close_pose")
    task.add(move)

    if task.plan():
        task.publish(task.solutions[0])

    time.sleep(100)
